// #include "ros/ros.h"
// #include "Person.h"
// #include <fstream>
// #include <sstream>

// #define TEST_COUNT 1000000

// int main(int argc, char **argv)
// {
//     ros::init(argc, argv, "msgs_talker");
//     ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME,
//                                    ros::console::levels::Debug);
//     ros::NodeHandle n;
//     ros::Publisher pub =
//         n.advertise<myproject::Person>("/msgs_test", 1000);
//     int count = 0;
//     ros::Rate loop_rate(10);
//     double time_used;
//     myproject::Person p;
//     p.name = "aoqiangl";
//     p.address = "安徽省阜阳市";
//     p.num = "123456789";
//     {
//         ros::Time start_time = ros::Time::now();
//         count = TEST_COUNT;
//         std::ofstream output("serial_data.dat", std::ios::binary);

//         while ((count--))
//         {
//             uint32_t serial_size = ros::serialization::serializationLength(p);
//             boost::shared_array<uint8_t> buffer(new uint8_t[serial_size]);
//             ros::serialization::OStream stream(buffer.get(), serial_size + 4);
//             ros::serialization::serialize(stream, p);
//             output.write(reinterpret_cast<char *>(buffer.get()), serial_size);
//         }

//         // 获得时间
//         ros::Time end_time = ros::Time::now();
//         time_used = (end_time - start_time).toSec() * 1000;
//         std::cout << "std_msgs serialization time is :" << time_used << "ms." << std::endl;
//         output.close();
//     }
//     {
//         myproject::Person p1;
//         ros::Time start_time = ros::Time::now();
//         count = TEST_COUNT;
//         std::ifstream input("serial_data.dat", std::ios::binary);
//         while((count--))
//         {
//             input.seekg(0,std::ios::end);
//             std::size_t file_size = input.tellg();
//             input.seekg(0,std::ios::beg);

//             boost::shared_array<uint8_t>buffer(new uint8_t[file_size]);
//             input.read(reinterpret_cast<char*>(buffer.get()),file_size);

//             ros::serialization::IStream stream(buffer.get(),file_size);
//             ros::serialization::deserialize(stream,p1);
//         }
//         ros::Time end_time = ros::Time::now();
//         time_used = (end_time - start_time).toSec() * 1000;
//         std::cout << "std_msgs deserialization time is :" << time_used << "ms." << std::endl;
//         input.close();
//     }
//     return 0;
// }

#if 0
//! 只是对rosmsg 单独的序列化性能进行测试
#include <iostream>
#include "ros/ros.h"
#include <chrono>

// Assume your ROS package name is 'myproject'
#include <myproject/Person.h> // Assume you have a ROS Person.msg message
#include <ros/serialization.h> // For ROS message serialization/deserialization

#define TEST_COUNT 1000000

int main(int argc, char **argv)
{
    ros::init(argc, argv, "ros_msg_test");
    ros::NodeHandle n;

    // Create ROS message instance and populate data (corresponding to Protobuf message content)
    myproject::Person ros_original_msg;
    ros_original_msg.name = "aoqingl";
    ros_original_msg.address = "安徽省阜阳市";
    ros_original_msg.num = "123456789";

    // --- ROS Native Message Serialization Benchmark ---
    std::cout << "--- ROS Native Message Serialization Benchmark ---" << std::endl;
    
    // Pre-allocate enough buffer space for all serializations.
    // The size won't change as we're only slightly modifying 'num'.
    uint32_t ros_serialized_size = ros::serialization::serializationLength(ros_original_msg);
    // Use a unique_ptr for better memory management than raw new/delete
    std::unique_ptr<uint8_t[]> ros_buffer_data_ptr(new uint8_t[ros_serialized_size]);
    uint8_t* ros_buffer_data = ros_buffer_data_ptr.get(); // Get raw pointer for OStream

    auto start_time_ros_serialize = std::chrono::high_resolution_clock::now();
    for (int i = 0; i < TEST_COUNT; ++i)
    {
        // Slightly change data for each iteration to prevent over-optimization
        ros_original_msg.num = std::to_string(i); 
        
        // CRITICAL FIX: Create a NEW OStream for each serialization
        ros::serialization::OStream ros_os(ros_buffer_data, ros_serialized_size);
        ros::serialization::serialize(ros_os, ros_original_msg);
    }
    auto end_time_ros_serialize = std::chrono::high_resolution_clock::now();
    std::chrono::nanoseconds duration_ros_serialize = 
        std::chrono::duration_cast<std::chrono::nanoseconds>(end_time_ros_serialize - start_time_ros_serialize);

    std::cout << "ROS Native Serialized Size (fixed): " << ros_serialized_size << " bytes" << std::endl;
    std::cout << "ROS Native Avg Serialization Time: " << (double)duration_ros_serialize.count() / TEST_COUNT << " ns" << std::endl;
    std::cout << "ROS Native Total Serialization Time: " << duration_ros_serialize.count() / 1e6 << " ms" << std::endl;

    // --- ROS Native Message Deserialization Benchmark ---
    std::cout << "\n--- ROS Native Message Deserialization Benchmark ---" << std::endl;
    
    // First, serialize one message to get the raw buffer data for deserialization.
    // Use a fresh OStream for this one-time serialization.
    ros_original_msg.num = "123456789"; // Restore original data for consistent deserialization benchmark
    ros::serialization::OStream temp_os(ros_buffer_data, ros_serialized_size);
    ros::serialization::serialize(temp_os, ros_original_msg);

    myproject::Person ros_deserialized_msg; // Object to store deserialized data

    auto start_time_ros_deserialize = std::chrono::high_resolution_clock::now();
    for (int i = 0; i < TEST_COUNT; ++i)
    {
        // CRITICAL FIX: Create a NEW IStream for each deserialization, always starting from the buffer's beginning
        ros::serialization::IStream ros_is(ros_buffer_data, ros_serialized_size);
        ros::serialization::deserialize(ros_is, ros_deserialized_msg);
    }
    auto end_time_ros_deserialize = std::chrono::high_resolution_clock::now();
    std::chrono::nanoseconds duration_ros_deserialize = 
        std::chrono::duration_cast<std::chrono::nanoseconds>(end_time_ros_deserialize - start_time_ros_deserialize);

    std::cout << "ROS Native Avg Deserialization Time: " << (double)duration_ros_deserialize.count() / TEST_COUNT << " ns" << std::endl;
    std::cout << "ROS Native Total Deserialization Time: " << duration_ros_deserialize.count() / 1e6 << " ms" << std::endl;

    return 0;
}
#endif 


#if 1
#include <ros/ros.h>
#include <chrono> // 用于高精度计时
#include <iostream>
#include <string>
#include <vector> // 用于存储每次的序列化时间
#include <numeric> // 用于 std::accumulate (如果使用)
#include <cmath>   // 用于 std::sqrt (如果计算标准差)

// 包含你的 ROS 原生消息头文件
// 替换为你的实际 ROS 包名和消息类型
#include <myproject/Person.h> // 例如：myproject/msg/Person.msg 生成的头文件

// 定义测试次数
#define NUM_PUBLICATIONS 1000 // 与 Protobuf 测试保持一致

int main(int argc, char** argv) {
    ros::init(argc, argv, "ros_native_publisher_node");
    ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME,
                                   ros::console::levels::Debug);
    ros::NodeHandle nh;

    // 声明你的 ROS 原生消息发布器
    ros::Publisher pub = nh.advertise<myproject::Person>("/my_ros_native_topic", 1000); // 队列大小 1000

    ros::Rate loop_rate(30); // 发布频率，每秒 5 次

    // 创建一个 ROS 原生消息实例并填充初始数据
    myproject::Person ros_msg_to_publish;
    ros_msg_to_publish.name = "Original_Robotinho";
    ros_msg_to_publish.address = "Original_Mars_Base_Alpha";
    ros_msg_to_publish.num = "0987654321";
    // 如果你的 Person.msg 包含更复杂的数据（例如数组），请在这里填充，
    // 确保与 Protobuf 消息的数据量和类型尽可能匹配，以进行公平对比。
    // ros_msg_to_publish.int_array.resize(50);
    // for (int i = 0; i < 50; ++i) {
    //     ros_msg_to_publish.int_array[i] = i * 10;
    // }
    // ros_msg_to_publish.float_array.resize(50);
    // for (int i = 0; i < 50; ++i) {
    //     ros_msg_to_publish.float_array[i] = i * 0.5f;
    // }


    std::vector<long long> serialization_times_ns; // 存储每次序列化的纳秒耗时
    serialization_times_ns.reserve(NUM_PUBLICATIONS); // 预分配内存

    int count = 0;
    while (ros::ok() && count < NUM_PUBLICATIONS) {
        ros_msg_to_publish.num = std::to_string(count); // 改变数据以模拟实际场景

        // --- 开始计时序列化 ---
        auto start = std::chrono::high_resolution_clock::now();

        pub.publish(ros_msg_to_publish);

        auto end = std::chrono::high_resolution_clock::now();
        // --- 结束计时序列化 ---

        std::chrono::nanoseconds duration = std::chrono::duration_cast<std::chrono::nanoseconds>(end - start);
        serialization_times_ns.push_back(duration.count()); // 记录本次时间

        count++;

        ros::spinOnce();
        loop_rate.sleep();
    }

    // --- 统计并打印结果 ---
    if (!serialization_times_ns.empty()) {
        long long total_duration_ns = 0;
        for (long long time_ns : serialization_times_ns) {
            total_duration_ns += time_ns;
        }

        double average_duration_ns = static_cast<double>(total_duration_ns) / serialization_times_ns.size();
        double average_duration_us = average_duration_ns / 1000.0;
        double average_duration_ms = average_duration_us / 1000.0;

        std::cout << "\n--- ROS 原生消息序列化性能测试结果 ---" << std::endl;
        std::cout << "总发布次数: " << NUM_PUBLICATIONS << std::endl;
        std::cout << "总序列化耗时: " << total_duration_ns / 1e9 << " 秒 (" << total_duration_ns << " 纳秒)" << std::endl;
        std::cout << "平均每次序列化耗时: " << average_duration_ns << " 纳秒" << std::endl;
        std::cout << "平均每次序列化耗时: " << average_duration_us << " 微秒" << std::endl;
        std::cout << "平均每次序列化耗时: " << average_duration_ms << " 毫秒" << std::endl;

        if (serialization_times_ns.size() > 1) {
            double sum_sq_diff = 0.0;
            for (long long time_ns : serialization_times_ns) {
                sum_sq_diff += std::pow(time_ns - average_duration_ns, 2);
            }
            double variance = sum_sq_diff / serialization_times_ns.size();
            double std_dev_ns = std::sqrt(variance);
            std::cout << "标准差: " << std_dev_ns << " 纳秒" << std::endl;
        }
    } else {
        std::cout << "没有进行任何发布操作。" << std::endl;
    }

    return 0;
}

#endif 
